#include "mccontrollerinterpreter.h"

using namespace KDL;

int main()
{
    int robotId = 1;
    QString robotName = "walle";
    McControllerInterpreter* testMc = new McControllerInterpreter;
    testMc->mcInit(robotId, robotName);
    testMc->startMove();
    std::cout << testMc->status << std::endl;
    testMc->cease();
    testMc->error();
    std::cout << testMc->status <<std::endl;
    testMc->setJogCoordinate(ENUM_USER);
    std::cout << testMc->currentJogCoordinate <<std::endl;

    ToolCoordinateInfo toolCoordinate;
    toolCoordinate.forwardTcpMatrix = Frame::Identity();
    toolCoordinate.inverseTcpMatrix = Frame::Identity();
    testMc->toolCoordinateList.append(toolCoordinate);
    UserCoordinateInfo userCoordinate;
    userCoordinate.forwardTcpMatrix = Frame::Identity();
    userCoordinate.inverseTcpMatrix = Frame::Identity();
    testMc->userCoordinateList.append(userCoordinate);

    std::cout << testMc->activateToolCoordinate(1) <<std::endl;
    std::cout << testMc->activateUserCoordinate(1) <<std::endl;

    PointPro pointIn,pointIn1;
    pointIn.jointFlag = 0;
    pointIn.ufValue = 1;
    pointIn.utValue = 1;
    pointIn.positionValue.push_back(22.0);
    pointIn.positionValue.push_back(41.0);
    pointIn.positionValue.push_back(38.0);
    pointIn.positionValue.push_back(16.0);
    pointIn.positionValue.push_back(32.0);
    pointIn.positionValue.push_back(64.0);
    pointIn1.jointFlag = 0;
    pointIn1.ufValue = 1;
    pointIn1.utValue = 1;
    pointIn1.positionValue.push_back(37.0);
    pointIn1.positionValue.push_back(43.0);
    pointIn1.positionValue.push_back(61.0);
    pointIn1.positionValue.push_back(13.0);
    pointIn1.positionValue.push_back(60.0);
    pointIn1.positionValue.push_back(80.0);
    std::cout << "test pointCoordinateToCartesian" <<std::endl;
    PointPro pointOut=testMc->pointCoordinateToCartesian(pointIn);
    for (int i=0; i<6; i++)
    {
        std::cout << pointOut.positionValue[i] << std::endl;
    }
    std::cout << "test pointCoordinateToJoint" <<std::endl;
    pointIn = testMc->pointCoordinateToJoint(pointOut);
    for (int i=0; i<6; i++)
    {
        std::cout << pointIn.positionValue[i] << std::endl;
    }


//---------test startMoveRun--------
    MoveParameter moveParameter;
    Frame pos;
    Twist vel;
    Twist acc;
    testMc->currentPos = Frame::Identity();
    testMc->currentPos.p = Vector(0.1, 0.2, 0.3);
//--------LINE & JOINT
////    moveParameter.moveType = ENUM_COMMAND_LINE;
//    moveParameter.moveType = ENUM_COMMAND_JOINT;
//    moveParameter.velocityUnit = ENUM_MMPS;
//    moveParameter.velocity = 200.0;
//    moveParameter.targetPoint = pointIn;
//    pos = testMc->pointProToFrame(pointIn);
//    std::cout << "frame of pointIn" << pos << std::endl;
//    moveParameter.cntRatio = 0;
//    moveParameter.acc = 100.0;
//    testMc->commandList.append(moveParameter);
//--------CIRCLE
    moveParameter.moveType = ENUM_COMMAND_CIRCLE;
    moveParameter.velocityUnit = ENUM_MMPS;
    moveParameter.velocity = 100.0;
    moveParameter.targetPoint = pointIn;
    pos = testMc->pointProToFrame(pointIn);
    std::cout << "frame of point1:" << std::endl << pos << std::endl;
    moveParameter.cntRatio = 0;
    moveParameter.acc = 100.0;
    testMc->commandQueue.append(moveParameter);
    moveParameter.targetPoint = pointIn1;
    testMc->commandQueue.append(moveParameter);
    pos = testMc->pointProToFrame(pointIn1);
    std::cout << "frame of point2:" << std::endl << pos << std::endl;
    testMc->startMoveRun();
    double pathTime = testMc->programTraj->Duration();
    std::cout << "program path time:" << pathTime << std::endl;
    std::ofstream op("./dat/pos.dat");
    std::ofstream ov("./dat/vel.dat");
    std::ofstream oa("./dat/acc.dat");
    for (double t=0.0; t<=pathTime; t+=0.1)
    {
        pos = testMc->programTraj->Pos(t);
        vel = testMc->programTraj->Vel(t);
        acc = testMc->programTraj->Acc(t);
        for (int i=0; i<4; i++)
        {
            for(int j=0; j<4; j++)
            {
                op << pos(i,j) << "\t";
            }
            op << "\n";
        }
        for(int i=0; i<6; i++)
        {
            ov << vel(i) << "\t";
        }
        ov << "\n";
        for(int i=0; i<6; i++)
        {
            oa << acc(i) << "\t";
        }
        oa << "\n";

    }
    op.close();
    ov.close();
    oa.close();

//--------test jogCoordinateRun
    testMc->setJogCoordinate(ENUM_WORLD);//手动坐标系改为世界
    std::cout << "number of joints:" << testMc->jointNum << std::endl;
    std::cout << "jog:" << testMc->jog(6,1,80) << std::endl;//寸动运行，正向，速度为80%
    std::cout << "jogAxis:" << testMc->jogAxis << std::endl;//寸动运行轴号
    std::cout << "jogVel:" << testMc->jogVel << std::endl;//寸动速度
   (PointPro&)testMc->currentCoordinate = pointOut;
    testMc->jogCoordinateRun();
    pathTime = testMc->jogTraj->Duration();
    std::cout << "jog path time:" << pathTime << std::endl;
    std::ofstream ojp("./dat/jogPos.dat");
    std::ofstream ojv("./dat/jogVel.dat");
    std::ofstream oja("./dat/jogAcc.dat");
    for (double t=0.0; (t<=3.0)&&(t<=pathTime); t+=0.1)
    {
        pos = testMc->jogTraj->Pos(t);
        vel = testMc->jogTraj->Vel(t);
        acc = testMc->jogTraj->Acc(t);
        for (int i=0; i<4; i++)
        {
            for(int j=0; j<4; j++)
            {
                ojp << pos(i,j) << "\t";
            }
            ojp << "\n";
        }
        for(int i=0; i<6; i++)
        {
            ojv << vel(i) << "\t";
        }
        ov << "\n";
        for(int i=0; i<6; i++)
        {
            oja << acc(i) << "\t";
        }
        oja << "\n";

    }
    ojp.close();
    ojv.close();
    oja.close();

//--------test getMotorCommand
    testMc->getMotorCommand();

}
